#define HAL_AD5245_10_EXTERN
#include "../includes.h"

#define I2C_SCL_LOW()       GPIO_ResetBits(GPIOC,  GPIO_Pin_4)
#define I2C_SCL_HIGH()      GPIO_SetBits(GPIOC,  GPIO_Pin_4)
#define I2C_SDA_LOW()       GPIO_ResetBits(GPIOC,  GPIO_Pin_3)
#define I2C_SDA_HIGH()      GPIO_SetBits(GPIOC,  GPIO_Pin_3)
#define I2C_SDA_IN()        GPIO_ReadInputDataBit(GPIOC,  GPIO_Pin_3)
#define printf 

static int uS = 10;

void set_uS(int us)
{
	uS = us;
}

//void delay_us(int us)
//{
//	us = us;
//}
void softReset();


static void I2C_DATA_OUT( )
{
	GPIO_InitTypeDef GPIO_InitStructure;
    
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOC, &GPIO_InitStructure);
}

static void I2C_DATA_IN( )
{
	GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOC, &GPIO_InitStructure);
}

/*******************************************************************************
函数: i2c_start()
功能: 产生一个开始状态
SDA: ￣￣\____
SCL: __/￣￣\__
参数: 
		None
返回: 
		None
*******************************************************************************/
void i2c_start()
{
	I2C_DATA_OUT();

	I2C_SCL_LOW();
	delay_us(uS);
	I2C_SDA_HIGH();
	delay_us(uS);
	I2C_SCL_HIGH();
	delay_us(uS);
	I2C_SDA_LOW();
	delay_us(uS);
	I2C_SCL_LOW();
	delay_us(uS);
}

/*******************************************************************************
函数: i2c_stop()
功能: 产生一个结束状态
SDA: ____/￣￣
SCL: __/￣￣\__
参数: 
		None
返回: 
		None
*******************************************************************************/
void i2c_stop( )
{
	I2C_DATA_OUT();

	I2C_SCL_LOW();
	delay_us(uS);
	I2C_SDA_LOW();
	delay_us(uS);
	I2C_SCL_HIGH();
	delay_us(uS);
	I2C_SDA_HIGH();
	delay_us(uS);
}

// Do one full clock cycle
//
// Changed 1/19/05 to eliminate one level of return stack requirements
//
void i2c_clock_cycle( )
{
	I2C_SCL_LOW();
	delay_us(uS);
	I2C_SCL_HIGH();
	delay_us(uS);
}

/*******************************************************************************
函数: i2c_ack_or_nack()
功能: 产生一个ACK或NACK
参数: 
		ack: ACK，TRUE输出低电平，FALSE输出高电平，即NACK
返回: 
		None
*******************************************************************************/
void i2c_ack_or_nack(u8 ack)
{
	I2C_SCL_LOW();
	I2C_DATA_OUT();
	delay_us(uS);
	(ack == TRUE) ? (I2C_SDA_LOW()) : (I2C_SDA_HIGH());
	delay_us(uS);
	I2C_SCL_HIGH();
	delay_us(uS);
	I2C_SCL_LOW();
	delay_us(uS);
}

/*******************************************************************************
函数: i2c_write_byte()
功能: 向 i2c slave 写入一个字节的数据
参数: 
		val: 将要写入的一个字节的数据
返回: 
		0: 写入成功；1: 写入失败
*******************************************************************************/
s32 i2c_write_byte(u8 val)
{
	int i;

	I2C_DATA_OUT();
	for (i = 0; i < 8; i++) /* 产生8个时钟周期，写入一个字节 */
	{
		I2C_SCL_LOW();
		delay_us(uS);
		((val & 1 << (7 - i)) == 0) ? (I2C_SDA_LOW()) : (I2C_SDA_HIGH());
		delay_us(uS);
		I2C_SCL_HIGH();
		delay_us(uS);
	}
	I2C_SCL_LOW();
	I2C_DATA_IN(); /* SDA调整为输入状态 */
	delay_us(uS);
	I2C_SCL_HIGH();
	while (i > 1) /* check ack */
	{
		delay_us(uS);
		(I2C_SDA_IN() > 0) ? (i--) : (i = 0);
	}
	I2C_SCL_LOW();
	delay_us(uS);
	I2C_DATA_OUT(); /* SDA调整为输出状态 */

	return i;
}

/*******************************************************************************
函数: i2c_read_byte()
功能: 从i2c slave中读取一个字节的数据
参数: 
		none
返回: 
		读取到的一个字节的数据
*******************************************************************************/
char i2c_read_byte()
{
	u32 i;
	u8 val = 0;

	I2C_SCL_LOW();
	delay_us(uS);
	I2C_DATA_IN(); /* SDA调整为输入状态 */
	for (i = 0x80; i; i = i >> 1)
	{
		I2C_SCL_HIGH();
		delay_us(uS);
		if (I2C_SDA_IN())
			val |= i;
		I2C_SCL_LOW();
		delay_us(uS);
	}
	I2C_DATA_OUT(); /* SDA调整为输出状态 */
	I2C_SDA_LOW();
	I2C_SCL_HIGH();
	delay_us(uS);
	I2C_SCL_LOW();
	delay_us(uS);
	return val;
}

/*******************************************************************************
函数: i2c_read_bytes()
功能: 从 i2c slave 中读取一串数据
参数: 
		buf: 接收缓冲区
		size: 需要读取的字节数
返回: 
		读取到的一个字节的数据
*******************************************************************************/
void i2c_read_bytes(u8 *buf, u32 size)
{
	u32 i;
	memset(buf, 0, size);
	if (size <= 0)
	{
		i2c_stop();
		return;
	}
	for (i=0; i<size; i++)
	{
		buf[i] = i2c_read_byte();
		// i2c_ack_or_nack(, TRUE);
	}
	// i2c_ack_or_nack(, FALSE);
	i2c_stop();

}

/*******************************************************************************
函数: i2c_write_bytes()
功能: 向 i2c slave 中写入一串数据
参数: 
		buf: 发送缓冲区
		size: 需要写入的字节数
返回: 
		none
*******************************************************************************/
int i2c_write_bytes(u8 *buf, u32 size)
{
	int i = 0;
	for (i=0; i<size; i++)
	{
		if (i2c_write_byte(buf[i]) != 0)
		{
			i2c_stop();
			return 1;
		}
	}
	return 0;
}

void i2c_init()
{
    GPIO_InitTypeDef GPIO_InitStructure;
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB , ENABLE);
    // 禁用JTAG，使能SW
    //GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
    
    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_3 | GPIO_Pin_4;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;		                              //IO口速度为50MHz
    GPIO_Init(GPIOC, &GPIO_InitStructure);
	
	I2C_SCL_HIGH();
	I2C_SDA_HIGH();
}

const uint8_t configBlock[135] = {
	0x29, 0x02, 0x10, 0x00, 0x28, 0xBC, 0x7A, 0x81, //8
	0x80, 0x07, 0x95, 0x00, 0xED, 0xFF, 0xF7, 0xFD, //16
	0x9E, 0x0E, 0x00, 0x10, 0x01, 0x00, 0x00, 0x00, //24
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x34, 0x00, //32
	0x28, 0x00, 0x0D, 0x0A, 0x00, 0x00, 0x00, 0x00, //40
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11, //48
	0x02, 0x00, 0x02, 0x08, 0x00, 0x08, 0x10, 0x01, //56
	0x01, 0x00, 0x00, 0x00, 0x00, 0xFF, 0x00, 0x02, //64
	0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x0B, 0x00, //72
	0x00, 0x02, 0x0A, 0x21, 0x00, 0x00, 0x02, 0x00, //80
	0x00, 0x00, 0x00, 0xC8, 0x00, 0x00, 0x38, 0xFF, //88
	0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x91, 0x0F, //96
	0x00, 0xA5, 0x0D, 0x00, 0x80, 0x00, 0x0C, 0x08, //104
	0xB8, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x10, 0x00, //112
	0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x0F, //120
	0x0D, 0x0E, 0x0E, 0x01, 0x00, 0x02, 0xC7, 0xFF, //128
	0x8B, 0x00, 0x00, 0x00, 0x01, 0x01, 0x40		//129 - 135 (0x81 - 0x87)
};

//Reads one byte from a given location
//Returns zero on error
uint8_t readRegister(uint16_t addr)
{
	unsigned char result;
	char buffer1[3] = {0};
	char buffer2[1] = {0};
	buffer1[0] = 0x52;
	buffer1[1] = addr >> 8;
	buffer1[2] = addr & 0x00FF;
	
	i2c_start();
	i2c_write_byte(buffer1[0]);
	i2c_write_byte(buffer1[1]);
	i2c_write_byte(buffer1[2]);
	i2c_stop();


	i2c_start();
	i2c_write_byte(0x53);
	buffer2[0] = i2c_read_byte();
	i2c_stop();
	result = buffer2[0];
	return (result);
}

//Reads two consecutive bytes from a given location
//Returns zero on error
uint16_t readRegister16(uint16_t addr)
{
	uint16_t result;
	char buffer[2] = {0};

	buffer[0] = readRegister(addr);
	delay_us(5);
	buffer[1] = readRegister(addr + 1);

	result = buffer[0] * 256 + buffer[1];

	return (result);
}

//Write a byte to a spot
bool writeRegister(uint16_t addr, uint8_t val)
{

	char buffer[4] = {0};
	buffer[0] = 0x52;
	buffer[1] = addr >> 8;
	buffer[2] = addr & 0x00FF;
	buffer[3] = val;

	i2c_start();
	i2c_write_byte(buffer[0]);
	i2c_write_byte(buffer[1]);
	i2c_write_byte(buffer[2]);
	i2c_write_byte(buffer[3]);
	i2c_stop();
	return (1); //All done!
}

//Write two bytes to a spot
bool writeRegister16(uint16_t addr, uint16_t val)
{
	writeRegister(addr, val >> 8);
	writeRegister(addr + 1, val & 0x00FF);
	return (1); //All done!
}

uint8_t begin()
{
	int counter = 0;
	uint16_t result = 0;
	uint16_t modelID = 0;
	// set_uS(10);
	// _deviceAddress = deviceAddress; //If provided, store the I2C address from user
	i2c_init();

	//Check the device ID
	modelID = readRegister16(VL53L1_IDENTIFICATION__MODEL_ID); // 
	printf("modelID = %x\n", modelID);

	//cout << modelID << endl;
	if (modelID != 0xEACC){}
		// return (false);

	softReset();

	//Polls the bit 0 of the FIRMWARE__SYSTEM_STATUS register to see if the firmware is ready
	while (readRegister16(VL53L1_FIRMWARE__SYSTEM_STATUS) & 0x01 == 0) // 
	{
		if (counter++ == 100)
			return (FALSE); //Sensor timed out
		delay_ms(2000);
	}

	//Set I2C to 2.8V mode. In this mode 3.3V I2C is allowed.
	result = readRegister16(VL53L1_PAD_I2C_HV__EXTSUP_CONFIG); // 
	result = (result & 0xFE) | 0x01;
	writeRegister16(VL53L1_PAD_I2C_HV__EXTSUP_CONFIG, result);

	return (TRUE); //Sensor online!
}


//Write the configuration block with a max of I2C_BUFFER_LENGTH bytes at a time
//Offset allows us to start at a location within the configBlock array
//This is the main function that setups up the VL53L1X to take measurements
//This was obtained by inspecting the example software from ST and by
//capturing I2C trace on ST Nucleo demo board

void startMeasurement(uint8_t offset)
{
	int x = 0;
	char buffer[135] = {0};

	for (x = 0; x < 135; x++)
	{
		buffer[x] = configBlock[x];
		writeRegister(0x01 + x, buffer[x]);
	}
	// i2c_start();
	// i2c_write_byte(0x52);
	// for (byte x = 0; x < 137; x++)
	// {
	// 	i2c_write_byte(buffer[x]);
	// }
	
	// i2c_stop();
}

void readMeasurement()
{
	int x = 0;
	char buffer[137] = {0};

	for (x = 0; x < 137; x++)
	{
		buffer[x] = readRegister(0x01 + x);
	}
}

//Polls the measurement completion bit
bool newDataReady()
{ //cout << "before data ready " << endl;
	uint16_t result;
	result = readRegister(VL53L1_GPIO__TIO_HV_STATUS);

	//cout << result << endl;
	//result =readRegister16(VL53L1_IDENTIFICATION__MODEL_ID);
	//cout << result << endl;
	if (readRegister(VL53L1_GPIO__TIO_HV_STATUS) != 0x03)
		return (TRUE); //New measurement!
					   // cout << "after data ready" << endl;
	return (FALSE);	//No new data
}

//Reset sensor via software
void softReset()
{
	writeRegister(VL53L1_SOFT_RESET, 0x00); //Reset
	delay_us(100);								//Driver uses 100us
	writeRegister(VL53L1_SOFT_RESET, 0x01); //Exit reset
}

//The sensor has 44 bytes of various datums.
//See VL53L1_i2c_decode_system_results() in vl53l1_register_funcs.c for the decoder ringer
//Start from memory address VL53L1_RESULT__INTERRUPT_STATUS
//0: result__interrupt_status,
//1: result__range_status, VL53L1_RESULT__RANGE_STATUS
//2: result__report_status, VL53L1_RESULT__REPORT_STATUS
//3: result__stream_count
//4: result__dss_actual_effective_spads_sd0
//6: result__peak_signal_count_rate_mcps_sd0
//8: result__ambient_count_rate_mcps_sd0
//10: result__sigma_sd0
//12: result__phase_sd0
//14: result__final_crosstalk_corrected_range_mm_sd0
//16: result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0
//18: result__mm_inner_actual_effective_spads_sd0
//20: result__mm_outer_actual_effective_spads_sd0
//22: result__avg_signal_count_rate_mcps_sd0
//24: result__dss_actual_effective_spads_sd1
//26: result__peak_signal_count_rate_mcps_sd1
//28: result__ambient_count_rate_mcps_sd1
//30: result__sigma_sd1
//32: result__phase_sd1
//34: result__final_crosstalk_corrected_range_mm_sd1
//36: result__spare_0_sd1
//38: result__spare_1_sd1
//40: result__spare_2_sd1
//42: result__spare_3_sd1
//44: result__thresh_info

//Get the 'final' results from measurement
uint16_t getDistance()
{
	return (readRegister16( VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0));
	//return readRegister16(VL53L1_IDENTIFICATION__MODEL_ID);
	//   return 100;
}

//Get signal rate
//This seems to be a number representing the quality of the measurement, the number of SPADs used perhaps
uint16_t getSignalRate()
{
	//From vl53l1_api.c line 2041
	uint16_t reading = readRegister16( VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0); // << 9; //FIXPOINT97TOFIXPOINT1616
	//float signalRate = (float)reading/65536.0;
	return (reading);
}




int measure()
{
    const int AVR_TIME = 32;
    long long tick_start;
	long long tick_stop;
    int distance = 0;
    int tmp = 0;
	int i = 0;

	startMeasurement(0);
	delay_ms(5);
    for (i = 0; i < AVR_TIME; i++)
    {
		tmp = getDistance(); //Get the result of the measurement from the sensor
        distance += tmp;
    }
    distance /= AVR_TIME;
    // printf("ranging time span = %.3llf ms\n", (tick_stop - tick_start) / 1e3);
    return distance;
}

int VX_init()
{
    bool status = begin();
    if (status) {
        printf("device initialize success.\n");
    } else {
        printf("device initialize failed.\n");
    }

    // t = Distance_Sensor.VL53L1_GetMeasurementTimingBudgetMicroSeconds()
    startMeasurement(0); //Write configuration bytes to initiate measurement
    delay_ms(2000);
    readMeasurement();
    measure();
}

